% scripts to use BPMD database
disp('Loading hdf5 file...');
data_set = h5load('logs.h5');
disp('Done.');

% Choose 10 frames here as an example 
START_STEP = 79;
END_STEP = 88;
MAX_STEP  = END_STEP - START_STEP + 1;

solution_info = struct();
% size: MAX_STEP X 5 [norml_error, slide_align, stick_residual, slide_cone_satisfy]
solution_info.itemize_error = zeros(MAX_STEP, 4);
% size: MAX_STEP X 3 [slide_badAlign, stick, slide]
solution_info.contact_counting = zeros(MAX_STEP, 3);
solution_info.iter = 0;

%% Compare the chosen frames 
for STEP = START_STEP : END_STEP
    solution_info.iter = solution_info.iter + 1;
    disp(STEP);
    CurrentFrame = sprintf('frame%06d', STEP);
    
    %% mNCP formulation 
    formulationModel = 'mNCP';
    solverName = 'prox_NCP';
    data_set.(CurrentFrame).dynamics = preDynamics(data_set.(CurrentFrame), formulationModel);
    data_set.(CurrentFrame).solution_info = solution_info;
    solution_info = simStepRun(data_set.(CurrentFrame), formulationModel, solverName);
    
end


%% plot the result 
disp('DONE Comparison, plotting result ...');

% Plot the error and the bar plot
solution_error = solution_info.itemize_error; 
contactError= solution_info.contact_counting;

FigHandle1 = figure;
set(FigHandle1, 'Position', [100, 100, 700, 500]);

subplot(2,1,1);
solution_error = solution_error(:, 1:3);
semilogy((START_STEP:1:END_STEP)', solution_error, 'LineWidth', 2);
legend('Nonpenetration error', 'Slide alignment error ', 'Stick residual velocity');

%legend('Nonpenetration error', 'Slide alignment error ', 'Stick residual velocity', 'slide cone satisfication');
ylim([10^(-6), 10^2]);
ylabel('Measured Error', 'FontSize', 12, 'FontWeight', 'bold');
title('Itemized error versus time step - NCP-PGS(prox)', 'FontSize', 12, 'FontWeight', 'bold');

subplot(2,1,2);
H = bar((START_STEP:1:END_STEP)', contactError,'stacked');
mycolor = {'r', 'b', 'g'};
for k=1:3
    set(H(k),'Facecolor',mycolor{k});
end
xlim([START_STEP-1, END_STEP+1]);
ylim([0, 300]);
legend('Slide: incorrect direction', 'Stick', 'Slide');
xlabel('Time step', 'FontSize', 12, 'FontWeight', 'bold');
ylabel('Num of Contacts', 'FontSize', 12, 'FontWeight', 'bold');
 
